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Why Particle Filters? 

Tool for tracking the state of a dynamic 
system modeled by a Bayesian network 
(Robot localization, SLAM, robot fault 
diagnosis) 

Similar applications to Kalman Filters, but 
computationally tractable for large/high- 
dimensional problems 

Key idea: Find an approximate solution 
using a complex model rather than an exact 
solution using a simplified model 


Why should you be interested in particle filters? 

Because, like Kalman filters, they’re a great way to track the state of a 
dynamic system for which you have a Bayesian model. That means that if you 
have a model of how the system changes in time, possibly in response to 
inputs, and a model of what observations you should see in particular states, 
you can use particle filters to track your belief state. 

Applications that we’ve seen in class before, and that we’ll talk about today, 
are Robot localization, SLAM, and robot fault diagnosis. 

So why should you use particle filters instead of Kalman filters? 

Well, the main reason is that for a lot of large or high-dimensional problems, 
particle filters are tractable whereas Kalman filters are not. 

The key idea is that a lot of methods, like Kalman filters, try to make problems 
more tractable by using a simplified version of your full, complex model. Then 
they can find an exact solution using that simplified model. But sometimes 
that exact solution is still computationally expensive to calculate, and 
sometimes a simplified model just isn’t good enough. So then you need 
something like particle filters, which let you use the full, complex model, but 
just find an approximate solution instead. 
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Demo of Robot Localization 



University of Washington Robotics and State Estimation 
Lab http://www.cs.washington.edu/ai/Mobile_Robotics/mcl/ 


What you see here is a demo from the University of Washington Robotics and 
State Estimation Lab. This is a frozen panel of the beginning of a robot 
localization task. The little blue circle is our best guess as to where the robot 
is now. The little red dots are different hypotheses for where the robot might 
be—at the beginning of the task, we have no idea where the robot is, so the 
hypotheses cover the entire space. As we’ll see later, each hypothesis is 
called a ‘particle’. The lines extending from the robot are sensor 
measurements taken by a laser rangefinder. The reason the lines extend well 
past the walls on the map is because the robot isn’t actually in that location. 
The robot movement comes from a person driving the robot manually; there is 
no automatic exploration going on. 


5 






Demo of Robot Localization 



University of Washington Robotics and State Estimation 
Lab http://www.cs.washington.edu/ai/Mobile_Robotics/mcl/ 


As you watch the animated gif, the best-guess location of the robot will jump 
around as the most likely hypothesis changes. As the robot moves and takes 
measurements, it figures out that most of the hypotheses it started with are 
pretty unlikely, so it gets rid of those. Pretty soon, the number of hypotheses 
is reduced to a few clouds in the hallway; the robot is actually in the hallway, 
but there’s a lot of symmetry there, so it’s not sure exactly where. Then it’s 
down to two hypotheses, and when the robot finally enters a room and looks 
around, it becomes clear that its current best hypothesis was actually correct. 
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Now I will discuss the formalization of the general problem that both particle 
filters and Kalman filters solve, which is called Bayes Filtering. 
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Bayes Filters 

Used for estimating the state of a 
dynamical system from sensor 
measurements 
Predict/update cycle 
Examples of Bayes Filters: 

- Kalman Filters 

- Particle Filters 


Bayes Filtering is the general term used to discuss the method of using a 
predict/update cycle to estimate the state of a dynamical system from sensor 
measurements. As mentioned, two types of Bayes Filters are Kalman filters 
and particle filters. 
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Bayes Filters cont. 

x state variable 
u inputs 
z observations 

d data (inputs and observations combined) 

Trying to find: belief about the current state 
P( x t I d 0 ...,) 

Given: u t , z t< perceptual model p(z t | x t ), 
action model p(x t | x M , u t J 


Now we introduce the variables we will be using. X is the state variable, and 
X t is the state variable at time t. U is the inputs to your system, z is the 
observations made by the sensors, and d just refers to inputs and 
observations together. What the Bayes Filter is trying to find at any point in 
time is the belief about the current state, which is the probability of x t given all 
the data we’ve seen so far. 

What we are given is the inputs, the observations, the perceptual model, which 
is the probability that you’ll see a particular observation given that you’re in 
some state at time t, and the action model, which is the probability that you’ll 
end up in state x, at time t, assuming that you started in state x,^ at time t-1, 
and input to your system. 
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Now I will give a quick review of robot localization and show what the problem 
is with doing localization with Kalman filters. 
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Robot Localization 


x = (x,y,0) 
motion model 
p(x, | x M , u t _j): 





perceptual model p(z t | x t ): 



So here’s the robot localization problem. You’re trying to track the state x, 
which is made up of the (x,y) position of the robot as well as its orientation, 
theta. 

You have a motion model for the robot, which looks like the two figures in the 
top right. If you start at the left end of the straight red line, pointed to the right, 
and tell your robot to move forward some distance, you expect it to end up 
somewhere in that cloud due to wheel slippage and the like. Darker regions 
have higher probability. If you start at the left end of the wiggly red line, your 
robot will have even more wheel slippage while turning (and it’s going a farther 
distance), and so the resulting position uncertainty cloud is larger. 

You also have a perceptual model for your robot, which is the probability that 
you’ll see certain observations when you’re in a particular state x t . On the 
bottom left is a picture of a robot in a map getting measurements from its laser 
rangefinders. Given a position and a map, you can use ray-tracing to get 
expected measurements for each rangefinder angle. Then you can look at a 
graph like the one on the bottom right, which is the result of characterizing 
your sensor. As you can see, for a particular expected distance, your sensor 
will give you a value near that distance with some reasonable probability. But 
rangefinders often miss objects and report seeing something at the maximum 
distance, so with some probability you expect the sensor to give you the max 
distance instead. So given an actual measurement and an expected distance, 
you can find the probability of getting that measurement using the graph. 
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The Problem with Kalman 
Filters in Robot Localization 


Kalman Filters only represent state 
variables as single Gaussians 
What if robot could be in one of two 
places? 
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The problem with Kalman filters is that they represent the state of the system 
using only single Gaussians. As you can see in the diagram excerpted from 
the demo just showed, sometimes it is necessary to have multimodal 
hypotheses about where the robot might be. If you can only choose one of the 
two possibilities (the most likely one), and you choose incorrectly, then it is 
extremely difficult to recover from your mistake. Particle filters, on the other 
hand, can keep track of as many hypotheses as there are particles, so if new 
information shows up that causes you to shift your best hypothesis completely, 
it is easy to do. 
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Now I will give an overview of the basic premise of particle filters. 
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Particle Filters 
(aka sequential Monte Carlo) 



Stale Space 


Represents pdf as a set of samples (particles) 
Each particle contains one set of values for 
the state variables 

Good for non-Gaussian, multi-modal pdfs 

Find an approximate solution using a complex 
model (arbitary pdf) rather than an exact 
solution using a simplified model (Gaussians) 


The basic idea of particle filters is that any pdf can be represented as a set of 
samples (particles). If your pdf looks like the two-humped line in the figure, 
you can represent that just by drawing a whole lot of samples from it, so that 
the density of your samples in one area of the state space represents the 
probability of that region. Each particle has one set of values for the state 
variables. This method can represent any arbitrary distribution, making it good 
for non-Gaussian, multi-modal pdfs. Again, the key idea is that you find an 
approximate representation of a complex model (any arbitrary pdf) rather than 
an exact representation of a simplified mode (Gaussians). 
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How to find samples 

• Want to sample from posterior, p(x t | d 0 t ) 
(call it p(x) for short) 

• But don't have explicit representation of 
full pdf to sample from 

• Can sample from prior belief (call it q(x)) 

• Sample from prior distribution 

• Update using observations: for each 
sample, compare p(x) to q(x) and adjust 
appropriately (find importance weights) 
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So what you actually want samples of is your posterior, which we will call p(x) 
for short. But how do you sample from your posterior? You don’t have an 
explicit representation of your posterior to draw points from. But you do know 
how to sample from your prior belief, because you had some belief from the 
last time step that you know how to update with your motion model. Let’s call 
the prior belief q(x). And you do know how to find, for any one x, what the 
posterior probability is, based on your prior belief and your observations. So, 
sample from q(x), and then for each sample that you made, update it using 
what we will call an ‘importance weight’, based on the observations made. 
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Sample Importance Resampling 



Slate Space 

Sample from prior be 
distribution) , 

_ J 

ief q(x) (for instance, the unifo 
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-m 

iltlYm. 

State Space 

Compute importance weights, w(x) 

Jillli 

= p(x) /q(x) 

Wifi 

IBL 


Stale Space 


Resample particles according to importance weights to get p(x) 

Samples with high weights chosen many times; density ref lects pdf 
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Here is a graphical visualization of the importance resampling process. Let’s 
say the posterior you’re trying to represent, as before, is the two-humped 
dotted line. Even if you have no information to start, and your prior is just the 
uniform distribution, you can still recover the properly sampled pdf of your 
posterior p(x). First you sample from your prior (the uniform distribution). For 
each of those samples, you can find the value of the posterior p(x). So for 
each sample, you assign that sample a weight, w(x), equal to p(x)/q(x). At this 
point, when the particles are weighted, you can use your highest-weighted 
(highest-probability) sample as your best-guess state, or you can use the 
weighted sum of particles to get a mean-equivalent, or you can use the 
average of particles within some distance from your best particle for a more 
intelligent best-guess. To represent the pdf properly with samples, though, we 
want the density of the particles in any segment of the state space to be 
proportional to the probability of that segment. As you can see in the middle 
panel, the particles are still spaced evenly from uniform sampling. So in order 
to adjust the densities properly, we resample the particles. That means we 
want to keep the total number of particles the same, while increasing the 
number of particles in the high-probability regions and decreasing the number 
of particles in low-probability regions. So we draw particles (with replacement) 
from the set of weighted particles according to their importance weights 
(probabilities). High-weighted particles can be chosen a lot of times, whereas 
low-weighted particles are likely not to be chosen at all. The result looks like 
the third figure, in which the particles go back to being unweighted, and the 
density of the particles properly represents the pdf. 


16 








Discrete Importance Resampling 
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Belief 
p(x) from 
last time step 

Prediction 
(sample from 
q(x)) 

Update 
(weight 
samples by 
importance) 

Resample 
according to 
weights to get 
p(x) for new 
time step 
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Another way to visualize the importance resampling process is to look at a 
discrete example. 

Let’s say you have a dynamic Bayes’ net with two states: Rain = true or Rain = 
false. You’re trying to figure out whether or not it’s raining, but you can’t see 
outside because you’re in an office with no windows. But let’s say that every 
hour, your boss stops by, and either he brings an umbrella, or he doesn’t. If 
he brings an umbrella, it’s likely raining, but maybe not, since some people 
bring umbrellas for no reason. Likewise, if he doesn’t bring an umbrella, it’s 
probably not raining, but it might be. So you have some model about the 
probability that it’s raining, given that you think it was raining an hour ago and 
your boss brings an umbrella, or doesn’t bring an umbrella, and so on. And 
you also have some model about the transition probabilities of rain/not-rain, 
saying that if it was raining an hour ago, it might have stopped with some 
probability, and so on. 

So we start, in the first column of boxes, Rairij, with some belief p(x) from the 
last time step. There are 8 particles in Rain=true and only 2 in Rain=false, 
meaning that p(rain=true) is 8/(2+8) = 4/5, and p(rain=false) is 2/(2+8) = 1/5. 
Next, we make a prediction about what the state will be in the next time step 
based on our transition model, before looking at any observations. This is our 
prior belief, q(x), and letting particles transition with some probability to each of 
the two states gives us the new sample set from q(x). Now we have 6 
particles in Rain=true, and 4 particles in Rain=false. 

Then let’s say the boss comes in, and he’s not carrying an umbrella. Now, we 
can find the probability of each particle based on our observation, according to 
our DerceDtual model. So the Rain=true Darticles have low Drobabilities. so we 




Why Resample? 

If you keep old particles around without 
resampling: 

- Particle depletion 

- Areas with high probability in posterior not 
represented well 

- Density of particles doesn't represent pdf 


So, just to make clear why it is necessary to resample the particles: 

If you just keep your old particles around forever without resampling them, 
what happens is that your particles drift around according to your motion 
model (transition probabilities for the next time step), but other than their 
weights, they are unaffected by your observations. Highly unlikely particles 
will be kept around and transitioned to more unlikely states, and you might 
only have say, one particle in the area of high probability of your posterior. So 
what you end up with is one particle with a way higher likelihood than any of 
the other particles, and a whole lot of particles with almost-nil probability. This 
is what we call ‘particle depletion’, because you in effect have only one 
particle. And one particle doesn’t represent a pdf very well. If you don’t have 
a lot of particles in the areas of your pdf with high probability, you won’t 
represent the pdf very well. The density of your particles should be high in 
high-probability areas, and low in low-probability areas. And so you have to 
resample the particles, so that they continue to represent the pdf accurately 
and keep track of many high-probability hypotheses, instead of tracking lots of 
useless, low-probability hypotheses. 
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Now we will show the particle filter algorithm step by step, using the example 
of robot localization. 
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The Algorithm 

• To start: Sample from initial pdf, p(x 0 ) 

- For localization, no idea where robot is -> 

throw particles everywhere! 

• For each time step, loop with three 
phases: 

1) Prediction 

2) Update 

3) Resample 


To start the algorithm, we need the initial belief state, p(x 0 ). This is just our 
initial guess of the pdf. For robot localization, if we have no idea, we can just 
scatter particles all over the map, as in the demo shown earlier. 

For each time step, we then loop with three phases: prediction, update, and 
resample. These will be explained in more detail in the next few slides. 
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Calculation of Belief for Robot Localization: 


Pdf of Perception Pdf from last 

current state model time step 

I 1 f I 

P( X t 1 d o t) = 'H P( Z t 1 X t) P( X t 1 u t-l» X t-l) P(*t-1 1 d 0 t-l) dx t-l 

I I 

Normalization Motion model 
constant 


Resample: Update: 

p(x) - the w(x) - the 

posterior importance 

probability, our weights 
belief about the 
state variables 


Prediction: 

q(x) - the prior probability 
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The equation on this slide shows the formalization of the steps taken in the 
particle filter algorithm. It is derived from applying Bayes rule to the posteior, 
and then using the Markov assumption. While executing the particle filter 
algorithm, we are calculating this equation from right to left. 

First we start with the pdf from the last time step, and then we multiply it by the 
motion model in the ‘prediction’ step to get q(x), the prior probability. The 
integral is there only to say that we can end up in the same state in time t from 
more than one state in time t-1, and thus we have to integrate over the states 
from time t-1. But we do not have to worry about this detail in the particle filter 
algorithm, since the particle representation takes care of the integral. 

Next, we find the importance weights w(x) using the perception model and 
normalize them so that they sum to 1. 

q(x) times w(x) = p(x), the posterior probability, which we use resampling 
based on the importance weights to achieve. 
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1) Prediction: for each particle, sample 


and add random, noisy values from 


action model 



Resulting proposal 



distribution (q(x)) 

: /C , 


approximates 



jp(x t 1 x t _j, u M ) p(x M | d 0 M ) d x t .j 
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In the prediction step, we take each particle and add a random sample from 
the motion model. In the figure, the robot starts from the lower left, and moves 
to the upper right. The resulting position, from the motion model, will be 
somewhere in that cloud of particles. The resulting distribution of particles 
approximates the prior distribution. 
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2) Update: each particle's weight is the 
likelihood of getting the sensor 
readings from that particle's 
hypothesis 


The weight associated 
with each particle is 
w(x) = p(x)/q(x) = p(z t | x t ), normalized so that 
all the weights sum to 1 
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During the update step, we take the sensor measurements and assign each 
particle a weight that is equal to the probability of observing the sensor 
measurements from that particle’s state. Those weights are then normalized 
so that they sum to 1. In the figure, the robot has observed the ‘stationary 
robot’ landmark at the top left, and based on that measurement, it has 
assigned weights to each particle. Darker particles have higher weights. 
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3) Resample: new set of particles are chosen 
such that each particle survives in 
proportion to its weight 


7 * /*\ 

Resulting distribution is p(x): 

P( x t I d 0 t ) = r\ p(z^ I xj j pfx, I x^) p t x^ I d 0 t-1 ) dx^ 

p(x) - the w(x) - the q(x) - the prior probability 
posterior importance 
probability weights 


Finally, in the resample step, a new set of particles is chosen so that each 
particle survives in proportion to its weight. As you can see in the picture, the 
weighted cloud of particles turns into the somewhat more condensed and 
smoother cloud of unweighted particles on the right. Highly unlikely particles 
at the fringe are not chosen, and the highly likely particles near the center of 
the cloud are replicated so that the high-probability region has a high density, 
correctly representing p(x), our posterior distribution. 
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Intro Summary 

• Particle f liters represent pdfs using 
sample densities 

• New belief can be found using the 
prediction-update-resample loop 

• Particle f liters can be better than 
Kalman fliters for robot localization 
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In this part, I will present you a particular application of particle filtering to the 
SLAM problem: Fast SLAM. 


I will show you how this algorithm applies the principles explained by Kaijen in 
her part, and how they differ somehow with these general principles. 


I will first review briefly the SLAM problem, since we have seen it in this class 
several weeks ago. 

I will then present you the classical solution to the SLAM problem and its main 
drawbacks. 

Then, I will explain you how FastSLAM works. 

Finally, I will end up with a short demo I wrote in Matlab. 
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Simultaneous Localization And 
Mapping: A review 

• Problem 

The robot has to create a map and localize 
itself on it 

• Framework 
Bayesian point of view: 

p(s t ,0lz t ,u t ,n t ) 


The SLAM problem consists of creating a map and localizing on it. 

The framework used for it is the bayesian point of view, as described by 
Kaijen. 

In the next slide, I will explain the nomenclature for FastSLAM. 
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Nomenclature 

U t 

Data: d t J 

Input to the robot 

1 < 

Measurement 

J s ‘ 

State: Xf i 

l * 

Position of the robot 

Positions of the landmarks 
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(The top part appears first) 

As Kaijen mentionned, a typical particle filtering algorithm takes some data, 
and outputs an estimate on the state of the system. 


In FastSLAM, the data are the same as in the localization problem described 
by Kaijen: they consist of both the input to the robot (go left, right, up, for 
instance), and the measurements at each time step (there is a landmark at 3 
feet in direction pi/4 with respect to the robot’s path). 


Now, the state is slightly different from the localization problem. Indeed, here, 
we want both to estimate the position of the robot and the positions of all the 
landmarks surrounding it. As a result, the state consists of both of them. Note 
that the landmarks are considered motionless in this case (no underscore ‘t’). 
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Demonstration: laser-based SLAM 
using particle filters 


From: http://www.cs.washington.edu/ai/Mobile_Robotics/mcl/ 
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Upfront, to show you guys how FastSLAM can perform, I show you a demo I 
found on the web. 

(I start the demo, and stop it after ten seconds to explain). 


On this demo, you see a robot moving in the corridor of a building, while 
mapping the walls, and localizing itself. 

The green dot represents the actual position of the robot. The red lines are the 
different guesses on the robot’s path (particles). The red dot, which we cannot 
see very well, is the best estimate on the robot’s position. Finally, we see the 
map of the building’s walls, as the robot moves along the corridors. 

(starting demo again) 


As the robot moves, you see the lines diverging, because the uncertainty on 
the robot’s position is increasing as time evolves. 

When the loop is closed, from the extra-information that the robot was actually 
at the same place a while ago, it can reduce the uncertainty (we see the lines 
gathering). 

Finally, we see how robustly the map is improved over time, as the robot 
moves. This is a feature of particle filtering: it is very robust. 
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(Introducing the second part of my talk: the classical solution to the SLAM 
problem.) 
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Classical SLAM solution 


• State: robot's & landmarks' positions 

• Algorithm: Extended Kalman Filter 

(ERF) 

• Outputs: positions and uncertainties 


31 


The classical SLAM solution uses: 


A *huge* state, consisting of the robot’s position, and all the landmarks’ 
positions, in a single huge vector 


The algorithm used is the extended kalman filter. We have already seen in this 
class the kalman filter. The extended kalman filter is the same algorithm , but 
extended to handle also non-linear models. 


The output of the algorithm is the estimate on this huge state, together with the 
uncertainty on it. 
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Issues with EKF-only SLAM 

• Gaussian assumption 

• Computational Complexity 

• Data Association 
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What are the main issues with this classical way of solving the SLAM problem 
? 


First, as Kaijen mentionned, the Extended Kalman Filter assumes that the 
probability density functions are gaussian, which may not be accurate. Since 
Kaijen mentionned it, I won’t go any further on this point. 


The second issue is the computational complexity of this approach (I will detail 
this point in the next slide) 


The third issue is the so-called ‘data association problem’, which I will also 
explain. 
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Complexity 


• Covariance matrix's size 0( K 2 ) 


Diagonal terms: 

Uncertainty on 
the robot’s 
landmarks’ 
positions 



Non diagonal terms: 

Cross-correlations 
between robot’s 
landmarks’ positions 


=>Cause: uncertainty on robot's pose 
correlates uncertainties on landmarks. 
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The reason why the classical solution to SLAm is complex is that, since the 
landmarks’ position uncertainty are correlated (I explain that in the next slide), 
the algorithm must keep track of all the cross-correlations between them (non¬ 
diagonal term of the covariance matrix, in green here). 

As a result, we must compute 0(K A 2) terms each time step. 

With as many as 10000 landmarks, this can be computationally unaffordable. 


33 




Why the robot's pose uncertainty 
correlates landmarks positions 

Ll 

Assume the measurement: l.2 

R 

A different assumption on the position of landmark 1 
leads to a different place of landmark 2, since the 
robot's pose is unknown 


Ll 



This slide explains in further detail why the landmark’s positions are correlated 
through the uncertainty on the robot’s position. I take an illustration. Supposing 
we have an observation from the robot (Ll, L2). Then, if we assume, for 
instance from another observation, a given position for Ll, through this 
observation, we end up to a given position for L2. Now, if the assumption on 
Ll is different, the conclusion on L2 is also. This is because the position of the 
robot is not known precisely. As a result, this lack of knowledge on the robot’s 
position correlates the positions of the landmarks. 
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Data Association 

• Which landmark are we observing ? 

• EKF assumes known. 

• Not robust to data association error. 
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In real application, we may not know which landmark we are observing (is it 
the one I have seen lOsec ago, or a new one ??), since many can have the 
same shape and color, and even because we may not be able to use an object 
recognition software. 

The problem with the EKF-SLAM is that it assumes that this association is 
known, and that it is very sensitive to errors in this association: it can diverge 
when an error is made. 
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Moving to the presentation of FastSLAM as a way to solve the drawbacks of 
the EKF SLAM. 
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FastSLAM : Principle 


one estimation of J2S 
positions*, 


and landmarks' 


=>M filters with known robot's position 
estimating only landmarks' positions 


=>Decorrelates landmarks' position uncertainties 
=>Reduces complexity 

=>Huge number of landmarks (50,000) can be 
handled 
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This slide presents the principle of particle-filtering slam, as opposed to EKF- 
SLAM. 


Instead of having ONE big estimation process on this huge state, keeping 
track of all the cross-correlations, 

(make the cross and second line appear) 

We replace it by M filters, each of which consider the robot’s position is 
perfectly known, and estimates the landmarks’ positions only: the great 
advantage of this idea is that it decorrelates the landmarks’ positions 
uncertainties. 
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The decorrelation of the 
landmarks' positions 

p(AnB) = p(A\B).p(B) 

p(s‘ ,A,|z f ,u' ,n‘) = ,z‘ ,u r ,n‘),u' ,n‘) 


Independence: 

p(s‘ ,u r ,n‘) = pCh^s 1 ,z { ,u f ,n t )p(s‘\z t ,u r ,n f ) 


Independent 
Extended Kalman Filters 

=> Complexity MK instead of K 2 


Particles 
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This slides shows the maths behind the decorrelation of the landmarks’ 
positions. 

Starting from computation of the probability density function of the state, we 
arrive to the structure of FastSLAM, in which one pdf is represented by the 
particles, and another, by the EKFs. 
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The Algorithm Step-by-step 

Propagate each particle by applying dynamic 
model to the robot's position 

Observe landmarks 

Generate data association hypothesis 
according to their likelihood 

Update landmarks' estimates for each particle 

Resample particles according to likelihood to 
make the observation 
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Here, I present step by step what the algorithm does. 

I go through each point in detail in the following slides. 
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• Propagate each particle by applying dynamic 
model to the robot's position 


Motion 
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This slide is a detailed explanation of the propagation point of the previous 
slide: when the robot moves, from the motion model, we know the posterior 
probability density function. We pick one particle to replace the former one, 
according to this pdf. 
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• Observe landmarks 



Actual position 

X \ 


of the Robot 




i ) 

'X 


Line of 



sight of the 



robot 
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Detailed explanation of the observation: the robot observes the landmarks in 
its line of sight. 
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Generate data association hypothesis 
according to their likelihood 


Computes the likelihoods of the observed 
landmark being each of the ones already seen, 
and of being a new one. 

Pick a data association hypothesis according to 
these likelihoods. 
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Detail of the data association hypothesis generation: each hypothesis is given 
a weight according to its likelihood, and each particle’s hypothesis is taken 
according to this weight. 
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• Update landmarks' estimates for each particle 



The update step following an observation: the 
estimated position of the landmark is 
modified, together with its uncertainty, for 
each particle. 
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Detail of the update step: after an observation, the robot updates its estimate 
concerning this landmark, and the uncertainty of this estimate. 


43 






• Resample particles according to likelihood to 

make the observation 


© • 

Resampling 


*#• 

Each particle is attributed a 

During the resampling step, 

weight according to the 

the best particles are 

likelihood to make the 

copied, and the worst are 

observation that was made 

deleted 
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Detail of the resampling step: during this step, the particles are given weights 
according to their likelihood (based on the observation that was made). Then, 
the best ones are selected and copied, while the worst ones are deleted. 
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Results 



From: Montemerlo et al., see bibliography 


This slide presents a first result obtained by Montemerlo’s team. It represents 
the evolution of the error in the positions of the robot and the landmarks as the 
number of landmarks increases: the robo’s position error decreases, and the 
landmarks’ error does not. 
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Results 



From: Montemerlo et al., see bibliography 


Another result from Montemerlo’s experiments. This is the evolution of the 
error in the positions of the robot and the landmarks as the number of particle 
increases. Interestingly enough, it does not change much as the number 
increases beyond 100. As a result, they chose to use 100 particles for their 
experiments. 
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Moving to the demo. 
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Demonstration: assumptions 


• Robot in 2D 

• Data association known 

• Landmarks motionless 

• Measurement: range & bearing 

• Motion model 

• 300 particles 
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This slide introduces my Matlab demo, stating the different assumptions I 
made for it. 
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demonstration: legend 

6 

Actual robot's position 

6 

Estimated robot's position 


Particles: robot's positions guesses 

( 6 ') 

Line of sight 

X 

Actual landmarks' positions 

© 

Estimated landmarks' positions 
and uncertainty (3-s area) 
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Here is the legend for the demo, since it contains many elements, and may be 
confusing at first glance. 
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to make you see 1 
and estimated poi 
and estimated -tc 













During the first steps, I represented two steps: after moving (the particles 
spread out because of the uncertainty in the motion), and after 
observation+update+resampling (the particles gather since only the best ones 
are selected through the resampling step.) 


Here, after moving, the update+resampling steps had taken place. As a result, 
the uncertainty on the landmarks’ positions is reduced for those within the line 
of sight. The number of particles is also reduced, since, through resampling, 
only the best ones are kept. 



































































































Last slide of the demo: the robot’s position is more accurately estimated, as 
we see from the spreading of the particles. The landmarks’ positions are also 
much more accurately estimated, as we see from the size of the blue circles 
around them. 
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FastSLAM Summary 

• Advantages 

s Handle much more landmarks 
S Generalization to multi-robot easier 
s More robust to unknown data-association 
problem 

• Drawbacks 

S Number of particles 
S Depletion 
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This slide recaps the pros and cons of the particle filtering applied to the SLAM 
problem. When presenting this slide, I mention the so-called ‘depletion’ 
problem, which consists of the number of particles going to zero through the 
resampling stage. 
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Challenges for Rovers 

Limited computational power 
Require realtime detection of faults 

- Avoid additional damage, waste of resources 

Large number of possible faults 

- Some faults only detectable using a series of 
observations over time 

Noisy sensors 

Imprecise or unreliable actuators 
Uncertain environment 
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(This slide is mostly self explanatory. I added the part below when giving the 
talk.) 


Some faults cannot be detected using a single set of observations. For 
example, it may be normal for a wheel to slip occasionally but repeated 
slipping could indicate a problem. 
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Fault Diagnosis as State Estimation 

• Hybrid discrete/continuous Hidden Markov 
Model 

• Discrete states represent operational modes 
and fault states 

- E.g. sending data, driving forward, right front 
wheel jammed, left rear wheel slipping, etc. 

• Continuous states represent observable state 
of the rover 

- E.g. wheel speed, motor current, tilt angle 

- Measurements may be noisy 
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Since we have so many sources of uncertainty, it’s natural for us to represent 
the problem using a probabilistic model. In this case, we choose a hybrid 
Hidden Markov Model where the discrete states correspond to functional 
modes and fault conditions in the rover. These are the hidden states in the 
model since we cannot measure them directly. 


The continuous states are the things that we *can* measure such as wheel 
speed or motor current. These states are needed because we assume that 
the sensors are noisy and therefore the observations are only approximations 
of this continuous state. 
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Fault Diagnosis as State Estimation 

• Construct a Bayesian Network (HMM) 

x dt = discrete state at time t 
x ct = continuous state at time t 
z, = observations at time t 

Bayesian Filter: 

P( x d , t ’X c , t \z 0 .. t ) = p(,z\x CJt ,x dt )\ Y<p( x c,t\ x ct-i’ x d,t)p( x d,t\ x d,t-i) dx c,t -1 

NB: Control inputs omitted for clarity 
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Now we can construct a Bayesian network using these states and the 
probabilities of transitioning between them. The equations are all similar to 
what was presented in the first part of the talk except that our state has both 
discrete and continuous components. The Bayesian filter equation changes 
slightly because the new discrete state depends only on the previous discrete 
state while the new continuous state depends on the previous continuous state 
and the new discrete state. Essentially, the discrete state changes 
independently while the continuous state depends on the discrete state. Note 
that I have omitted the control inputs from my equations for clarity but they can 
be added in just as they were in the earlier equations. 
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Fault Diagnosis as State Estimation 

• Given previous state and new 
observations, determine new state 

- Probability distribution over states 

- Uses sequence of observations to detect 
dynamic faults 

- Bayesian Filter is intractable (exponential) 
in general case 
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Using this type of model, we are able to track the current state and thereby 
watch for faults. At each time step, we calculate the new state based on the 
previous state and a new set of observations. However, by doing fault 
detection using a Hidden Markov Model, we actually get something better. We 
get a probability distribution across all the possible states. This is convenient 
since it feeds directly into certain types of planners such as the POMDP 
planners discussed in an earlier lecture. The Hidden Markov Model also 
incorporates a series of observations into its estimate allowing us to detect 
those dynamic faults I mentioned. However, the problem is that there is no 
closed-form solution to the Bayesian filter equation when using arbitrary 
probability distributions. 
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Particle Filters to the Rescue 

• Particle filter finds an approximate solution 
by "sampling" the probability distribution 

W1 stuck 

W2 stuck 

0% ( ] Normal 

i ) 25% 

_ ^7 • • \ 

1 • ) 

W1 broken /-K. 

\ W2 broken 

5% f • j 

" • • • 55% 

V • • • • J 
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So, naturally, we’ll use particle filters to find an approximate solution. Here is a 
simple example model where we have one “normal” state and four “fault” 
states. To find the probability that we’re in a particular state, we simply count 
the particles in that state and divide by the total number of particles in the 
system. 
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Hybrid State Particle Filter 

1. Create particles by first sampling from the 

discrete state distributi on p(x d0 ),then from the continuous 
state distributi on given the discrete state p(x c0 |jc^). 

2. During prediction step first update discrete state : 

x d!t p( x d,,\ x d!t-^ 

then continuous state: 

x c,l p( x c,i\^d!t>^c!t-i) 

3. Calculate particle weights as: 

w ( t ° = p(y\x%,^ t ) 

81 


The calculations for the particle filter are the same as those presented earlier 
except for the way the two different types of state are handled. Again, the 
continuous states are dependent on the discrete state. Therefore, when we 
create the initial particles, we sample from the discrete state prior distribution 
first and then sample from the continuous state distribution given the discrete 
state that we selected. Similarly, during the prediction step of the algorithm, 
we update the discrete state first and then update the continuous state based 
on the new discrete state. The importance weights are calculated using the 
entire state, as before. 
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Particle Filter Advantages 

Can adjust number of particles to match 
available computational resources 

- Tradeoff between accuracy of estimate and 
required computation 

Computationally tractable even with complex, 
non-linear, non-Gaussian models 

- Approximate solution to complex model vs. 
exact solution to approximate model 
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What do we gain by using a particle filter approach to fault diagnosis? 


First, we can easily adjust the amount of computation that we need to do. By 
increasing or decreasing the number of particles in the system, we can trade 
off the accuracy of our estimates for faster results. If we reduce the number of 
particles, we have less work to do but our approximation will not be as close to 
the correct answer. In fact, we can even adjust the number of particles on-the- 
fly as conditions permit. For example, a rover may be able to change the 
frequency of its processor depending on the amount of power that’s available. 
Then, when a lot of light is falling on its solar panels, it can use a lot of 
particles and get an accurate approximation and when it’s cloudy it can scale 
back to fewer particles and reduce its accuracy. 

This works because we’ve retained the complex model of the system and are 
just approximating a solution with varying degrees of accuracy. This is in 
contrast to approaches where the model is simplified (e.g. by assuming 
Gaussian distributions) to deal with the intractability. In that case you’re stuck 
with whatever approximations you’ve made and can’t improve your accuracy 
on-demand. 


For these reasons, particle filtering seems to be a good fit for rover 
applications. The complex model can be created offline, before the rover is 
sent out, and the rover need only perform the relatively simple particle update 
calculations in the field. Once it is deployed, the rover can tailor its accuracy 




Particle Filter Problem 

Very few particles in improbable states 

- Lag time, high variance 

- Faults are usually highly improbable!!! 

Increase the number of particles to 
make sure all states are represented 

- Increases computational requirements 

- Lots of computation wasted on "normal" 
states 
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However, there’s one problem with using particle filters and that is that 
improbable states have very few or no particles. This can create two 
problems. First, there can be a delay between when an event occurs and 
when the corresponding state becomes likely. Normally, when something 
happens, our observations will cause the particles in a particular state to be 
weighted more heavily and they will multiply. However, if that state has no 
particles in it, we have to wait for at least one particle to randomly transition 
into it before it can start multiplying. This can take a long time if that particular 
transitional probability is very low. Second, we can get a high variance in the 
estimate for a state because the probability represented by a single particle is 
larger that the probability we’re trying to estimate. Therefore, it will tend to flip 
back and forth between zero and one particles rather than settling on a 
consistent intermediate value. Of course, the reason these are problems is 
that faults are usually improbable. 


The obvious solution is to increase the number of particles so that each 
particle represents a smaller probability. The problem with this is that you may 
need an enormous number of particles to represent very small probabilities. 
This will drastically increase the amount of computation you have to do. Plus, 
all of the states will get more particles so most of the additional computation 
will be wasted updating particles in the states that already had lots of particles. 


(Note: I considered adding another slide to better explain the lag time problem 
but everyone that I asked about it said that they got it just fine from what I 
said.) 
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Particle Filter Enhancements 

• How can we ensure that improbable states are 
represented without using lots of particles? 

- Risk Sensitive Particle Filter 

• Increase number of particles in “risky" or high cost 
states 

- Variable Resolution Particle Filter 

• Group similar states together to pool their particles 

• Break them apart if fault occurs 

“Particle Filters for Rover Fault Diagnosis," V Verma, S Gordon, R 
Simmons, S Thrun, Robotics and Automation Magazine, June 2004 
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There are various approaches to addressing this problem in the literature. I’m 
going to show you two approaches presented by Verma et al. in the paper 
reference here. 


The goal of these enhancements is to make sure that all the states in a system 
are represented by at least a few particles while still keeping the total number 
of particles small. 


(The rest of the slide is pretty self-explanatory.) 
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Risk Sensitive Particle Filter 

Fault states get few particles but the cost of 
miscalculating their probability is high 

- "Normal" states get lots of particles but we don't 
need a highly accurate estimate 

Solution: Add a risk function to bias sampling 
towards high cost states 

- Particle filter samples from product of original 
distribution and risk function 

- Divide by risk function to find original distribution 
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(Self-explanatory) 
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RSPF Example 


Without Risk Function 

Normal 


P = 0*0.05 P= 1*0.05 


With Risk Function 

Normal 



3*0.05 p= 5*0.05 

r{Xwi broh , n ) r i X Wlbroken ) 


Here’s a simple example of the effect of the risk function. Without the risk 
function, the normal state gets almost all of the particles while the fault states 
are poorly represented. The estimated probability of each state can be 
calculated directly from the number of particles in each state. 


With the risk function, the sampling is biased so that the fault states get more 
particles. We then need to divide by the bias in order to get the actual 
probability distribution that we are trying to estimate. 
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RSPF Risk Function 

Risk function: maps a discrete state to a positive 
real number 

Ex : r( x Normal ) = 1 

T ( X Wlbroken ) = ^0 

r{x wlhroken ) = 100 


«" = p{y, k?.*£) 


(,■>_ r(x 'J!,> 


r(4‘. Li) 




• Choosing a good risk function is important but also 
somewhat difficult (open researc h topic) 
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The risk function simply maps a discrete state to a positive number. As an 
example, we might assign a value of “1” to the normal state. Then we assign 
larger values to the fault states to help ensure that they get enough particles. 
Let’s say the W1 is a drive wheel and W2 is a steering wheel. If W1 were to 
break (and we didn’t notice) we would probably just stop. Whereas if W2 were 
to break and we didn’t notice, we might veer off in a random direction and 
drive off a cliff. Therefore, we assign a higher value to W2 because it could 
have a higher cost if we miss it. 


The top arrow shows the distribution that we were originally trying to estimate 
on the left and the new distribution that we want to estimate on the right. 
Gamma is a normalization constant which ensures that the expression on the 
right is a probability distribution. To switch from the first distribution to the 
second, we only need to draw our original samples from the product of the risk 
function and the initial distribution and then modify the importance weight 
calculation as shown by the second arrow. 


Clearly, the key to making this work well is finding a good risk function. 
Unfortunately, this is still a topic of active research so there is currently no 
accepted way to do this. An expert may be able to guess fairly decent risk 
function. The authors also referred to another paper which used a POMDP 
planner to create a risk function based on the potential future cost of 
inaccurately tracking each state. 




RSPF Experimental Setup 

Discrete States 

10) W4 gear broken 
9) W3 gear broken 
8) W4 stuck 
7) W3 stuck 
6) W2 stuck 
5) W1 stuck 
4) W4 motor broken 
3) W3 motor broken 
2) W1 or W2 broken 
1) Normal 

Time Step 
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Now, I ‘II talk about an experiment from the paper where they compared a risk 
sensitive particle filter to a classical one. In this case, they were modeling a 
four wheeled rover with 10 discrete states; one normal state and nine fault 
states. The states are arbitrarily numbered. The graph shows the actual state 
of the robot as it changes during the simulation. It starts in the normal state 
and then, at time step 17, wheel 3 becomes stuck against a rock. They then 
tell the robot to back up, wheel 3 becomes unstuck and the rover operates 
normally until time step 30 where the gear on wheel 4 breaks. It then remains 
broken for the rest of the simulation. 
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RSPF Results 


100 particles 1000 particles 10.000 particles 100.000 particles 
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The top row of each set shows the estimated state of the robot during the 
simulation. Here we see only the most likely state, not the full distribution. 
The bottom row shows the error in the estimate; “0” if the estimate is correct 
and “1” if it is wrong. 


The classical particle filter does very poorly with anything less than 100,000 
particles, either missing faults completely or detecting the wrong fault. With 
100,000 particles, the estimate is close but there is still a lag between the fault 
occurring and the rover detecting it. 


With the risk sensitive particle filter, the estimate is already very good with only 
100 particles and is perfect with 1000 or more particles. The authors did show 
the actual risk function used for this experiment. They only say that it was 
derived “heuristically.” 
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Variable Resolution Particle Filter 

• Many faults have similar symptoms 

- E.g. Any stuck wheel on the right creates 
a pull to the right 

• Group these states 
together so that the 
new state has higher 
probability and gets 
more particles 




This enhancement is based on the observation that some faults have similar 
symptoms. In this case, we can group those faults together into one abstract 
state that contains the individual fault states. This hierarchical model is 
constructed ahead of time by the rover designer and therefore relies on an 
expert to appropriate group states together. In this example, we see a simple 
model for a six-wheeled rover. There is one normal state (ND) and one fault 
state for each of the six wheels being stuck. The states are labeled with an “L” 
or “R” for “left” or “right” and an “F”, “M” or “R” for “front”, “middle” or “rear.” In 
the higher-level model, all the “R” states are combined to form an abstract 
“RS” (for “right side”) state. The same is done of the left side. 


Without using the variable resolution particle filter, the fault states are likely to 
have few or no particles. In this situation, when a fault does occur, the rover 
will likely pick the wrong fault. For example, if a fault occurred in one of the 
right wheels while there was only a particle in RM (as in the diagram on the 
left), the RM particle would multiply and the rover would decide that the fault 
was in RM. This happens because the symptoms for all the right side wheels 
are similar. 


When the states for the left and right sides are grouped together, it effectively 
pools the particles from the individual states. This makes it more likely that the 
abstract state will be well-represented. It also means that the rover does not 
immediately try to pick a specific fault when something happens. Instead it 
detects that some fault has occurred but delays the determination of the 
SDecific fault. 
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Variable Resolution Particle Filter 


When a fault occurs, 
the individual states 
will become more 
probable 

- Can now break them 
apart and still have 
sufficient particles in 
each 



Once the rover has determined that a fault has occurred in one of the abstract 
states, it can switch to a more refined model for that state to determine exactly 
where the fault occurred. The particles from the large abstract state are 
distributed to the individual states according to their prior probabilities and the 
algorithm continues. In this example, since sufficient particles have 
accumulated in RS it is decomposed into RF, RM and RR and its particles are 
distributed among them. 
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VRPF Results 
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These results were collected using a model similar to the one in the previous 
slides. Many simulations were run with varying numbers of particles. For 
each experiment the KL divergence between the estimated probability 
distribution and the actual probability distribution was computed. (A particle 
filter with 1,000,000 particles was used as the actual distribution.) For large 
numbers of particles, both the classical and variable resolution particle filters 
performed comparably. This is because there were sufficient particles to 
populate the faults states, even without using the variable resolution 
enhancement. However, for small numbers of particles, the variable resolution 
particle filter had much lower divergence as well as lower variance (as 
indicated by the error bars). 
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Summary 

Rover fault diagnosis is challenging 

Particle fliters find an approximate solution 
to a complex HMM problem 
- Allow non-linear, non-Gaussian models 

Classical PFs have difficulty accurately 
estimating low-probability states 

"Risk Sensitive" and "Variable Resolution" 

PFs can dramatically reduce the number of 
particles needed to get a good answer 
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In summary, fault diagnosis is difficult for rovers. The models can be very 
complex and the computational resources are very limited. Particle filters 
allow us to find an approximate solution with whatever resources are available. 
However, naively applying particle filtering to fault diagnosis can lead to poor 
tracking of the most important states. Risk sensitive and variable resolution 
particle filters are two techniques that can dramatically improve fault detection 
while keeping the computational complexity low. 
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Recap 

Particle filters find approximate solutions to 
Bayesian Network problems by: 

- Sampling the state space 

- Updating samples using transition probabilities 

- Weighting the samples based on observations 

- Resampling based on the weights 


Hopefully in this lecture you’ve gotten an idea of how particle filters work and 
the types of problems they can solve efficiently. 

(Reiterate each step.) 
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Recap 

Many applications including SLAM and fault 
diagnosis 

Efficient means of solving complex problems 

- Large numbers of states 

- Arbitrary probability distributions 

- Tradeoff between accuracy and computation 

Key idea: Find an approximate solution using a 
complex model rather than an exact solution 
using a simplified model 
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We’ve shown you SLAM and fault diagnosis but there are also many other 
applications of particle filters. 


They are particularly useful when your (Bayesian network) problem has a large 
number of states or requires arbitrary probability distributions either in the 
results or in the model. They can also be useful when you want to be able to 
tradeoff (possibly on-the-fly) between accuracy and computational complexity. 
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